#include <iostream>
#include <sstream>

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/centroid.h>
#include <pcl/common/angles.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

using namespace std;

int main(int argc, char const *argv[])
{
    if (argc < 2)
    {
        cerr << "no input file. please pass file path as parameter." << endl;
    }

    // load point cloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile(argv[1], *cloud);
    // cout << cloud->width << " " << cloud->height << endl;

    pcl::PassThrough<pcl::PointXYZ> pass;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 1.0);
    pass.filter(*cloud_filtered);
    // cout << cloud_filtered->width << " " << cloud_filtered->height << endl;

    pcl::PCLPointCloud2::Ptr cloudToBeDownsampled(new pcl::PCLPointCloud2());
    pcl::PCLPointCloud2::Ptr cloudDownsampledTemp(new pcl::PCLPointCloud2());
    pcl::toPCLPointCloud2(*cloud_filtered, *cloudToBeDownsampled);
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud(cloudToBeDownsampled);
    sor.setLeafSize(0.01f, 0.01f, 0.01f);
    sor.filter(*cloudDownsampledTemp);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudDownsampled(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(*cloudDownsampledTemp, *cloudDownsampled);
    // cout << cloudDownsampled->width << " " << cloudDownsampled->height << endl;

    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
    seg.setAxis(Eigen::Vector3f(0.0, 1.0, 0.0));
    seg.setEpsAngle(pcl::deg2rad(7.0));
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.015);
    pcl::ExtractIndices<pcl::PointXYZ> extract;

    vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> planeClouds;
    vector<pcl::ModelCoefficients::Ptr> coefficients;

    for (int i = 0; i < 2; i++)
    {
        coefficients.emplace_back(new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
        seg.setInputCloud(cloudDownsampled);
        seg.segment(*inliers, *coefficients[i]);

        if (inliers->indices.size() < 10)
        {
            cerr << "planar " << i + 1 << " failed" << endl;
            return -1;
        }

        // Extract the planar inliers from the input cloud
        planeClouds.emplace_back(new pcl::PointCloud<pcl::PointXYZ>());
        extract.setInputCloud(cloudDownsampled);
        extract.setIndices(inliers);
        extract.setNegative(false);
        extract.filter(*planeClouds[i]);

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRest(new pcl::PointCloud<pcl::PointXYZ>);
        extract.setNegative(true);
        extract.filter(*cloudRest);
        cloudDownsampled = cloudRest;
    }

    auto z = [&coefficients](int i) { return -coefficients[i]->values[3] / coefficients[i]->values[2]; };

    if (z(0) > z(1))
        swap(planeClouds[0], planeClouds[1]);

    float rad = atan2(coefficients[0]->values[0], coefficients[0]->values[2]);
    if (rad > pcl::deg2rad(90.0))
        rad -= pcl::deg2rad(180.0);
    if (rad < pcl::deg2rad(-90.0))
        rad += pcl::deg2rad(180.0);
    cout << "angle: " << pcl::rad2deg(rad) << " degree" << endl;
    Eigen::Vector2f unitNormalVector(sin(rad), cos(rad));
    Eigen::Vector2f unitDirectionVector(cos(rad), -sin(rad));

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(planeClouds[1]);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.015);
    ec.setMinClusterSize(150);
    ec.setMaxClusterSize(3000);
    ec.setSearchMethod(tree);
    ec.setInputCloud(planeClouds[1]);
    ec.extract(cluster_indices);

    vector<Eigen::Vector4f> centroids;
    for (auto &is : cluster_indices)
    {
        pcl::PointCloud<pcl::PointXYZ> box;

        extract.setNegative(false);
        extract.setIndices(pcl::PointIndices::Ptr(new pcl::PointIndices(is)));
        extract.setInputCloud(planeClouds[1]);
        extract.filter(box);

        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(box, centroid);
        centroids.push_back(centroid);
    }

    // a在xz平面上投影的向量
    auto v = [](Eigen::Vector4f &a) { return Eigen::Vector2f(a.x(), a.z()); };

    auto closestPoint = *min_element(centroids.begin(), centroids.end(), [&unitDirectionVector, &v](Eigen::Vector4f &a, Eigen::Vector4f &b) {
        return abs(v(a).dot(unitDirectionVector)) < abs(v(b).dot(unitDirectionVector));
    });

    auto vc = v(closestPoint);
    cout << "x: " << vc.dot(unitDirectionVector) << endl;
    cout << "z: " << vc.dot(unitNormalVector) << endl;

    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor(0.0, 0.0, 0.5);
    viewer.addCoordinateSystem(1.0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red(planeClouds[0], 255, 0, 0);
    viewer.addPointCloud(planeClouds[0], red, "1");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green(planeClouds[1], 0, 255, 0);
    viewer.addPointCloud(planeClouds[1], green, "2");
    // viewer.addPlane(*coefficients1, "p1");
    // viewer.addPlane(*coefficients2, "p2");
    int i;
    for (auto &c : centroids)
    {
        pcl::PointXYZ p(c.x(), c.y(), c.z());
        cout << p << endl;
        stringstream idStr;
        idStr << "centroid " << i++;
        viewer.addSphere(p, 0.01, idStr.str());
    }
    pcl::PointXYZ p1(closestPoint.x(), closestPoint.y(), closestPoint.z());
    pcl::PointXYZ p2(p1.x - unitNormalVector.x(), p1.y, p1.z - unitNormalVector.y());
    viewer.addLine(p1, p2);
    viewer.spin();

    return 0;
}
